'''
Created on Aug 15, 2019

@author: modys
'''
import numpy as np
import matplotlib.pyplot as plt
class PID():
    '''
    classdocs
    '''


    def __init__(self ,time_end,sample_time,Kp, Ki, Kd):
        '''
        Constructor
        '''
        self._Kp = Kp;
        self._Ki = Ki;
        self._Kd = Kd;
        
        self._max_output = 0.0;
        self._min_output = 0.0;
        self._sample_time = sample_time;
        
        self._error =0.0;
        self.P_term =0.0;
        self.I_term = 0.0;
        self.D_term =0.0;
        self._output = 0.0;
        
        # Data Saving for results plotting
        self.t_data = np.arange(0,time_end,sample_time);
        self._e_intgeral_data = np.zeros_like(self.t_data);
        self._e_derivative_data = np.zeros_like(self.t_data);
        self._output_data = np.zeros_like(self.t_data);
        self._error_data = np.zeros_like(self.t_data);
        
    def output_limits(self):
        return self._min_output, self._max_output;
    
    def _clamp(self,value, limits):
        lower, upper = limits
        if value is None:
            return None
        elif upper is not None and value > upper:
            return upper
        elif lower is not None and value < lower:
            return lower
        return value
        
    def update(self,set_point,current_val,i):
        self._error = set_point - current_val;
        
        self.P_term =  self._Kp * self._error;
        
        self.D_term =  self._Kd *(self._error)/self._sample_time;
        
        self.I_term += self._Ki * self._error * self._sample_time;
        self.I_term = self._clamp(self.I_term, self.output_limits)
        
        self._output =self.P_term + self.D_term + self.I_term
        self._output = self._clamp(self._output, self.output_limits)
            
        self._e_intgeral_data[i] = self._Ki * self.I_term;
        self._e_derivative_data[i] = self._Kd * self.D_term;
        self._output_data[i] = self._output;
        self._error_data[i] = self._Kp * self._error;
            
        return (self._output);
    
    def plotResults(self):
        plt.figure(1)
        plt.title('intgeral')
        plt.plot(self.t_data, self._e_intgeral_data)
        plt.legend()
        plt.grid()
        
        
        plt.figure(2)
        plt.title('derivative')
        plt.plot(self.t_data, self._e_derivative_data)
        plt.legend()
        plt.grid()
        
        
        plt.figure(3)
        plt.title('error')
        plt.plot(self.t_data, self._error_data)
        plt.legend()
        plt.grid()
        
        
        plt.figure(4)
        plt.title('output')
        plt.plot(self.t_data, self._output_data)
        plt.legend()
        plt.grid()
        
        plt.show()
        